package ros_bag

import (
	"github.com/lherman-cs/go-rosbag"
	"io"
	"iron_man_comm/src/common/log"
	"iron_man_comm/src/ros_bag/msg"
	"os"
)

type RosBagParser struct {
	filePath    string
	msgCallback func(msg msg.TopicData)
	errCallback func(topic string, err error)
	topicMap    map[uint32]string
	pauseFlag   bool
	cancelFlag  bool
}

func (r *RosBagParser) ParserBag(filePath string, dataCallback func(msg msg.TopicData), errCallback func(topic string, err error)) error {
	msg.Init()
	r.filePath = filePath
	r.msgCallback = dataCallback
	r.errCallback = errCallback
	r.pauseFlag = false
	r.cancelFlag = false

	r.topicMap = make(map[uint32]string)

	file, err := os.Open(filePath)
	if err != nil {
		return err
	}
	defer file.Close()

	log.Info("open rosbag pass, %s", filePath)

	decoder := rosbag.NewDecoder(file)

	return r.parserData(decoder)
}

func (r *RosBagParser) PauseParse() {
	r.pauseFlag = true
}

func (r *RosBagParser) ResumeParse() {
	r.pauseFlag = false
}

func (r *RosBagParser) CancelParse() {
	r.cancelFlag = true
}

func (r *RosBagParser) parserData(decoder *rosbag.Decoder) error {
	closeFlag := true
	for {
		if r.cancelFlag {
			log.Info("cancel parse")
			break
		}
		if r.pauseFlag {
			continue
		}
		record, err := decoder.Read()
		if err == io.EOF {
			log.Info("read end")
			break
		}
		closeFlag = true
		switch record := record.(type) {
		case *rosbag.RecordMessageData:
			r.parserMsg(record)
			break
		case *rosbag.RecordConnection:
			r.parserConnection(record)
			break
		case *rosbag.RecordBase:
			break
		case *rosbag.RecordBagHeader:
			break
		case *rosbag.RecordChunk:
			break
		case *rosbag.RecordChunkInfo:
			r.parserChunkInfo(record)
			break
		case *rosbag.RecordIndexData:
			break
		default:
			log.Error("unknown record type")
			closeFlag = false
			break
		}
		if closeFlag {
			record.Close()
		}
	}
	log.Info("parse done")
	return nil
}

func (r *RosBagParser) parserMsg(record *rosbag.RecordMessageData) {
	connId, err := record.Conn()
	if err != nil {
		log.Error("read msg conn id fail, err: %s", err.Error())
		return
	}
	topicName, ok := r.topicMap[connId]
	if !ok {
		log.Error("no topic found, conn id: %d", connId)
		return
	}
	r.parserMsgByTopic(record, topicName)
}

func (r *RosBagParser) parserConnection(record *rosbag.RecordConnection) {
	topic, err := record.Topic()
	if err != nil {
		log.Error("read topic fail, err: %s", err.Error())
		return
	}
	connId, err := record.Conn()
	if err != nil {
		log.Error("read conn id fail, err: %s", err.Error())
		return
	}
	topicName, ok := r.topicMap[connId]
	if ok {
		log.Warn("conn id: %d is exist, %s -> %s", connId, topicName, topic)
	}
	r.topicMap[connId] = topic
}

func (r *RosBagParser) parserChunkInfo(record *rosbag.RecordChunkInfo) {
	log.Info("parser chunk info")
	startTime, err := record.StartTime()
	if err != nil {
		log.Error("read start time fail, err: %s", err.Error())
		return
	}
	endTime, err := record.EndTime()
	if err != nil {
		log.Error("read end time fail, err: %s", err.Error())
		return
	}
	count, err := record.Count()
	if err != nil {
		log.Error("read count fail, err: %s", err.Error())
		return
	}
	pos, err := record.ChunkPos()
	if err != nil {
		log.Error("read chunk pos fail, err: %s", err.Error())
		return
	}
	log.Info("record chunk info, time: %s ~ %s, count: %d, chunk pos: %d",
		startTime.Format("2006-01-02 15:04:05.000"), endTime.Format("2006-01-02 15:04:05.000"), count, pos)
}
